#include "read_kaist_data.h"

double ns_to_seconds(uint64_t nanoseconds) {
    const uint64_t ns_per_second = 1000000000ULL;
    uint64_t whole_seconds = nanoseconds / ns_per_second;
    uint64_t fraction_ns = nanoseconds % ns_per_second;
    return whole_seconds + static_cast<double>(fraction_ns) / ns_per_second;
}

read_data::read_data(const std::string& path, const SensorDataType& sensor_data_type) : sensor_data_type_(sensor_data_type) {
    path_ = path;
    origin_timestamp = 0.0;
    if(sensor_data_type_ == SensorDataType::KAIST) {
        ext_rot_vehicle2laser_ << 0.00130201, 0.796097, 0.605167, 0.999999, -0.000419027, -0.00160026, -0.00102038, 0.605169, -0.796097;
        ext_trans_vehicle2laser_ << 0.8349,-0.0126869,1.76416;      
    }

    if(sensor_data_type_ == SensorDataType::NCLT) {
        ext_rot_vehicle2laser_  = eulerToRotationMatrix(180.0f,0.0f,0.0f);
        ext_trans_vehicle2laser_ << 0.28, 0.0, -0.44;  
    }
    // std::cout << "ext_rot_vehicle2laser_ : \n" << ext_rot_vehicle2laser_ << std::endl;
    // std::cout << "ext_trans_vehicle2laser_ : \n " << ext_trans_vehicle2laser_ << std::endl;

    undistortion_ptr_ = new Undistortion(path);

    Tib_ = Eigen::Matrix4d::Identity();

    Tib_(0,3) = 0.07;
    Tib_(2,3) = -1.7;


    std::vector<std::vector<double>> T_wheel_cam = {
        {-0.00680499, -0.0153215, 0.99985, 1.64239},
        {-0.999977, 0.000334627, -0.00680066, 0.247401},
        {-0.000230383, -0.999883, -0.0153234, 1.58411},
        {0.0, 0.0, 0.0, 1.0}
    };

    for (int i = 0; i < 4; ++i) {
        for (int j = 0; j < 4; ++j) {
            Tbc_(i, j) = T_wheel_cam[i][j]; 
        }
    }

    Tbg_ = Eigen::Matrix4d::Identity();

    Tbg_(0,3) = 0.247401;
    Tbg_(1,3) = -0.20;
    Tbg_(2,3) = -1.96;
}

std::vector<std::string> read_data::split(std::string str, std::string pattern) {
	std::string::size_type pos;
	std::vector<std::string> result;
	str += pattern;
	int size = str.size();

	for (int i = 0; i < size; i++) {
		pos = str.find(pattern, i);

		if (pos < size) {
			std::string s = str.substr(i, pos - i);
			result.push_back(s);
			i = pos + pattern.size() - 1;
		}
	}
	return result;
}

bool read_data::LoadSensorData(const std::string& encoder_file_path, std::unordered_map<std::string, std::string>* time_data_map) {
    std::ifstream encoder_file(encoder_file_path);
    if (!encoder_file.is_open()) {
        std::cout << "[LoadSensorData]: Failed to open encoder file." << std::endl;
        return false;
    } 

    std::string line_str, time_str;
    while (std::getline(encoder_file, line_str)) {
        std::stringstream ss(line_str);
        if (!std::getline(ss, time_str, ',')) {
            std::cout << "[LoadSensorData]: Find a bad line in the encoder file.: " << line_str << std::endl;
            return false;
        }
        
        // std::cout << "[main]: time_str : " << time_str << std::endl;
        // std::cout << "[main]: line_str : " << line_str << std::endl;

        time_data_map->emplace(time_str, line_str);
    }

    return true;
}

bool read_data::read_only_kaist_data(std::unordered_map<std::string, std::string>& time_encoder_map, 
                              		 std::unordered_map<std::string, std::string>& time_imu_map,
                              		 std::unordered_map<std::string, std::string>& time_gps_map, 
                              		 std::unordered_map<std::string, std::string>& time_vrs_gps_map,
                                     std::unordered_map<std::string, std::string>& time_sick_middle_map,
                                     std::unordered_map<std::string, std::string>& time_velodyne_left_map) {
	bool state = true;
	// Load all encoder data to buffer.
    if (!LoadSensorData(path_ + "sensor_data/encoder.csv", &time_encoder_map)) {
        std::cout << "[main]: Failed to load encoder data." << std::endl;
        state = false;
    } 

    // Load all imu date to buffer.
    if (!LoadSensorData(path_ + "sensor_data/xsens_imu.csv", &time_imu_map)) {
        std::cout << "[main]: Failed to load imu data." << std::endl;
        state = false;
    } 

    // Load all gps data to buffer.
    if (!LoadSensorData(path_ + "sensor_data/gps.csv", &time_gps_map)) {
        std::cout << "[main]: Failed to load gps data." << std::endl;
        state = false;
    } 

    if (!LoadSensorData(path_ + "sensor_data/vrs_gps.csv", &time_vrs_gps_map)) {
        std::cout << "[main]: Failed to load gps data." << std::endl;
        state = false;
    } 	

    if (!LoadSensorData(path_ + "sensor_data/SICK_middle_stamp.csv", &time_sick_middle_map)) {
        std::cout << "[main]: Failed to load SICK_middle data." << std::endl;
        state = false;
    }  

    if (!LoadSensorData(path_ + "sensor_data/VLP_left_stamp.csv", &time_velodyne_left_map)) {
        std::cout << "[main]: Failed to load VLP_left_stamp data." << std::endl;
        state = false;
    }  
    
    std::ifstream file_data_stamp(path_ + "sensor_data/data_stamp.csv");
    if (!file_data_stamp.is_open()) {
        std::cout << "[main]: Failed to open data_stamp data." << std::endl;
    }

    std::vector<std::string> line_data_vec;
    line_data_vec.reserve(3);
    std::string line_str, value_str;

	std::vector<double> times;

    while (std::getline(file_data_stamp, line_str)) {
        line_data_vec.clear();
        std::stringstream ss(line_str);
        while (std::getline(ss, value_str, ',')) { line_data_vec.push_back(value_str); }

        const double kToSecond = 1e-9;
        const std::string& time_str = line_data_vec[0];

        const double timestamp = std::stod(time_str) * kToSecond;
    	times.push_back(timestamp);
    	if(times.size() > 200) {
    		break;
    	}
    }

    std::sort(times.begin(),times.end());
    
    origin_timestamp = times[0] - 10.0;

    return state;
}

std::unordered_map<std::string, std::string> read_data::read_only_nclt_data(const std::string& file_path) {
    bool state = true;
    std::unordered_map<std::string, std::string> time_and_sensor_type_map;
    // Load all encoder data to buffer.
    if (!LoadSensorData(path_ + file_path, &time_and_sensor_type_map)) {
        std::cout << "[main]: Failed to load encoder data." << std::endl;
        state = false;
    } 

    return time_and_sensor_type_map;
}

OriginFeaturePoint::OriginFeaturePoint(int _x, int _y, float _score) : 
    x(_x), y(_y), score(_score) {
    for (int i = 0; i < 128; ++i) {
        descriptor_data[i] = 0.0f;
    }
}

bool read_data::LoadFrameData(std::vector<FeatureOutput>& results,
                   std::vector<cv::KeyPoint>& undistorted_kps, const std::string& filename) {
    std::ifstream infile(filename, std::ios::binary);
    if (!infile.is_open()) {
        std::cerr << "Error: Could not open feature file for reading." << std::endl;
        return false;
    }
    
    // 清空输出容器
    results.clear();
    undistorted_kps.clear();
    
    // 读取时间戳
    size_t time_len;
    infile.read(reinterpret_cast<char*>(&time_len), sizeof(size_t));
    std::string time_str(time_len, '\0');
    infile.read(&time_str[0], time_len);
    
    // 读取特征检测结果数量
    size_t results_size;
    infile.read(reinterpret_cast<char*>(&results_size), sizeof(size_t));

    // std::cout << " result size =========== : " << results_size << std::endl;

    for (size_t i = 0; i < results_size; ++i) {
        FeatureOutput output;
        
        // 读取索引
        size_t index;
        infile.read(reinterpret_cast<char*>(&index), sizeof(size_t));
        
        // 读取时间戳
        infile.read(reinterpret_cast<char*>(&output.timestamp.sec), sizeof(uint32_t));
        infile.read(reinterpret_cast<char*>(&output.timestamp.nsec), sizeof(uint32_t));
        
        // 读取全局特征
        infile.read(reinterpret_cast<char*>(output.global_feature), 256 * sizeof(float));

        // for(size_t idx = 0; idx < 128; ++idx) {
        //     std::cout << " output.global_feature =========== : " << idx << ", " << output.global_feature[idx] << std::endl;
        // }

        // 读取特征点数量
        size_t fp_size;
        infile.read(reinterpret_cast<char*>(&fp_size), sizeof(size_t));
        
        // std::cout << " fp_size =========== : " << fp_size << std::endl;

        // 读取特征点坐标和分数
        for (size_t j = 0; j < fp_size; ++j) {
            float x, y, score;
            infile.read(reinterpret_cast<char*>(&x), sizeof(float));
            infile.read(reinterpret_cast<char*>(&y), sizeof(float));
            infile.read(reinterpret_cast<char*>(&score), sizeof(float));
            output.fp_vector.emplace_back(x, y, score);

            // std::cout << " feature point =========== : " << x << " , " << y << " , " << score << std::endl;

        }

        // 读取局部描述子
        for (size_t j = 0; j < fp_size; ++j) {
            infile.read(reinterpret_cast<char*>(output.fp_vector[j].descriptor_data), 128 * sizeof(float));

            // for(size_t idx = 0; idx < 128; ++idx) {
            //     std::cout << " output.fp_vector[j].descriptor_data_ =========== : " << output.fp_vector[j].descriptor_data_[idx] << std::endl;
            // }
        }
        
        results.push_back(output);
    }
    
    // 读取去畸变关键点数量
    size_t kps_size;
    infile.read(reinterpret_cast<char*>(&kps_size), sizeof(size_t));
    
    // 读取去畸变关键点
    for (size_t i = 0; i < kps_size; ++i) {
        cv::KeyPoint kp;
        infile.read(reinterpret_cast<char*>(&kp.pt.x), sizeof(float));
        infile.read(reinterpret_cast<char*>(&kp.pt.y), sizeof(float));
        infile.read(reinterpret_cast<char*>(&kp.response), sizeof(float));
        undistorted_kps.push_back(kp);

        // std::cout << " feature point =========== : " << kp.pt.x << " , " << kp.pt.y << " , " << kp.response << std::endl;

    }
    
    infile.close();
    return true;
}

std::vector<feature::FeaturePoint> read_data::ConvertFeatureOutputToFeaturePoints(
    const std::vector<FeatureOutput>& results, const std::vector<OriginFeaturePoint>& filter_features,
    const std::vector<cv::KeyPoint>& undistorted_kps, const Eigen::Matrix3d& intrinsic_matrix) {
    
    std::vector<FeaturePoint> feature_points;

    double fx = intrinsic_matrix(0,0);
    double fy = intrinsic_matrix(1,1);
    double cx = intrinsic_matrix(0,2);
    double cy = intrinsic_matrix(1,2);

    for (size_t i = 0; i < results.size(); ++i) {
        const FeatureOutput& output = results[i];
        
        for (size_t j = 0; j < output.fp_vector.size(); ++j) {
            const OriginFeaturePoint& origin_fp = output.fp_vector[j];
            const OriginFeaturePoint& resize_fp = filter_features[j];
            feature::FeaturePoint new_fp;
            
            // 转换图像坐标
            new_fp.image_point << 1.0*resize_fp.x, 1.0*resize_fp.y;
           
            // LOG(INFO) << " image_point ===== : " << new_fp.image_point.transpose() << std::endl;

            // 转换描述符
            for (int k = 0; k < 128; ++k) {
                new_fp.descriptor(k) = origin_fp.descriptor_data[k];

                // LOG(INFO) << " origin_fp.descriptor_data[k] ===== : " << origin_fp.descriptor_data[k] << std::endl;
            }
            // LOG(INFO) << " new_fp.descriptor ===== : " << new_fp.descriptor.transpose() << std::endl;

            // 设置分数
            new_fp.score = origin_fp.score;
            
            // 转换全局特征
            for (int k = 0; k < 256; ++k) {
                new_fp.global_feature(k) = output.global_feature[k];
            }

            cv::KeyPoint undistorted_kp = undistorted_kps[j];
    
            new_fp.undistorted_point << undistorted_kp.pt.x, undistorted_kp.pt.y;
            new_fp.normalized_point << (undistorted_kp.pt.x-cx)/fx, (undistorted_kp.pt.y-cy)/fy, 1.0;

            // Vector2d predict_undistorted_point;
            // predict_undistorted_point << fx*new_fp.normalized_point[0]+cx, fy*new_fp.normalized_point[1]+cy;

            // LOG(INFO) << " predict_undistorted_point ===== : " << predict_undistorted_point.transpose() << std::endl;
            // LOG(INFO) << " new_fp.preundistorted_point ===== : " << new_fp.undistorted_point.transpose() << std::endl;

            // LOG(INFO) << " new_fp.normalized_point ===== : " << new_fp.normalized_point.transpose() << std::endl;

            feature_points.push_back(new_fp);
        }
    }

    return feature_points;
}

Eigen::Matrix4d read_data::read_extrinsic() {
    std::vector<std::vector<double>> T_imu_cam = {
        {-0.00681, -0.01532, 0.99987, 1.71239},
        {-0.99998, 0.00033, -0.00680, 0.24740},
        {-0.00023, -0.99988, -0.01532, -0.11589},
        {0.0, 0.0, 0.0, 1.0}
    };

    // std::vector<std::vector<double>> T_imu_cam = {
    //     {-0.0070035, -0.0149880,  0.9998631,   1.712},
    //     {-0.9999750,  0.0010519, -0.0069885,   0.247},
    //     {-0.0009470, -0.9998871, -0.0149950,  -0.116},
    //     {0.0, 0.0, 0.0, 1.0}
    // };
    Eigen::Matrix4d T;
    for (int i = 0; i < 4; ++i) {
        for (int j = 0; j < 4; ++j) {
            T(i, j) = T_imu_cam[i][j]; 
        }
    }
    return T;
}

std::vector<FeatureOutput> read_data::LoadFrameTxtData(std::vector<FeatureOutput>& results, 
        std::vector<cv::KeyPoint>& undistorted_kps,
        const std::string& filename) {

    std::ifstream infile(filename);
    if (!infile.is_open()) {
        std::cerr << "Error: Could not open feature file." << std::endl;
        return {};
    }


    FeatureOutput current_output;

    // 读取文件头
    std::string header;
    std::getline(infile, header);
    if (header.substr(0, 3) != "===") {
        std::cerr << "Error: Invalid file format." << std::endl;
        return {};
    }

    while (!infile.eof()) {
        std::string line;
        std::getline(infile, line);
        if (line.empty()) continue;

        // LOG(INFO) << "line : " << line << std::endl;

        if (line.find("FeatureOutput:") == 0) {

            // LOG(INFO) << "line : " << line << std::endl;
            // LOG(INFO) << "current_output.fp_vector : " << current_output.fp_vector.size() << std::endl;
            continue;

        } else if (line.find("Timestamp:") == 0) {
            // 读取时间戳
            // std::istringstream ss(line);
            // ss >> current_output.timestamp.sec >> current_output.timestamp.nsec;
            current_output.timestamp.sec = 0;
            current_output.timestamp.nsec = 0;

            // LOG(INFO) << "line : " << line << std::endl;

            // LOG(INFO) << "current_output.timestamp : " << current_output.timestamp.sec << ", " << current_output.timestamp.nsec << std::endl;

        } else if (line.find("Global Feature") == 0) {
            // 读取全局特征
            std::string feature_line;
            std::getline(infile, feature_line);
            std::istringstream iss(feature_line);
            for (int i = 0; i < 256; ++i) {
                iss >> current_output.global_feature[i];
                // LOG(INFO) << "current_output.global_feature[i] : " << current_output.global_feature[i] << std::endl;
            }

            // LOG(INFO) << "line : " << line << std::endl;

        } else if (line.find("Feature Points") == 0) {
            // 读取特征点数量
            int count;
            infile >> count;
            // current_output.fp_vector.resize(count);

            // 读取特征点数据
            for (int i = 0; i < count; ++i) {
                int x, y;
                float score;
                infile >> x >> y >> score;
         
                current_output.fp_vector.push_back(OriginFeaturePoint(x,y,score));
                
                // LOG(INFO) << "point : " << x << ", " << y << ", " << score << std::endl;
            }
        } else if (line.find("local descriptor") == 0) {
            int count;
            infile >> count;

            for (int i = 0; i < count; ++i) {
                std::string feature_line;
                std::getline(infile, feature_line);
                std::istringstream iss(feature_line);
                for (int j = 0; j < 128; ++j) {
                    iss >> current_output.fp_vector[i].descriptor_data[j];
                    // LOG(INFO) << "current_output.fp_vector[i].descriptor_data[j] : " << current_output.fp_vector[i].descriptor_data[j] << std::endl;
                }
            }
        } else if (line.find("Undistorted KeyPoints") == 0) {
            int count;
            infile >> count;
            undistorted_kps.resize(count);

            for (int i = 0; i < count; ++i) {
                cv::KeyPoint key;
                infile >> key.pt.x >> key.pt.y >> key.response;
                undistorted_kps[i] = key;
            }

            // LOG(INFO) << "count : " << count << std::endl;
            // LOG(INFO) << "line : " << line << std::endl;
        }

        // LOG(INFO) << "line : " << line << std::endl;
    }

    // 添加最后一个结果
    if (!current_output.fp_vector.empty()) {
        results.push_back(current_output);
    }

    infile.close();
    // LOG(INFO) << "undistorted_kps : " << undistorted_kps.size() << std::endl;
    return results;
}

// 转换函数
std::vector<feature::FeaturePoint> read_data::ConvertTxtFeatureOutputToFeaturePoints(
    const std::vector<FeatureOutput>& results, 
    const std::vector<cv::KeyPoint>& undistorted_kps, const Eigen::Matrix3d& intrinsic_matrix) {
    
    std::vector<FeaturePoint> feature_points;

    // LOG(INFO) << " intrinsic_ ===== :\n " << intrinsic_ << std::endl;

    double fx = intrinsic_matrix(0,0);
    double fy = intrinsic_matrix(1,1);
    double cx = intrinsic_matrix(0,2);
    double cy = intrinsic_matrix(1,2);
    double image_width_scale_ratio = undistortion_ptr_->image_width_scale_ratio_;
    double image_height_scale_ratio = undistortion_ptr_->image_height_scale_ratio_;

    for (size_t i = 0; i < results.size(); ++i) {
        const FeatureOutput& output = results[i];
        
        for (size_t j = 0; j < output.fp_vector.size(); ++j) {
            const OriginFeaturePoint& origin_fp = output.fp_vector[j];
            
            feature::FeaturePoint new_fp;
            
            // 转换图像坐标
            new_fp.image_point << origin_fp.x*image_width_scale_ratio, origin_fp.y*image_height_scale_ratio;
           
            // LOG(INFO) << " image_point ===== : " << new_fp.image_point.transpose() << std::endl;

            // 转换描述符
            for (int k = 0; k < 128; ++k) {
                new_fp.descriptor(k) = origin_fp.descriptor_data[k];

                // LOG(INFO) << " origin_fp.descriptor_data[k] ===== : " << origin_fp.descriptor_data[k] << std::endl;
            }
            // LOG(INFO) << " new_fp.descriptor ===== : " << new_fp.descriptor.transpose() << std::endl;

            // 设置分数
            new_fp.score = origin_fp.score;
            
            // 转换全局特征
            for (int k = 0; k < 256; ++k) {
                new_fp.global_feature(k) = output.global_feature[k];
            }

            cv::KeyPoint undistorted_kp = undistorted_kps[j];
      
            // new_fp.undistorted_point << undistorted_kp.pt.x/image_width_scale_ratio, undistorted_kp.pt.y/image_height_scale_ratio;
            // new_fp.normalized_point << (undistorted_kp.pt.x/image_width_scale_ratio-cx)/fx, (undistorted_kp.pt.y/image_height_scale_ratio-cy)/fy, 1.0;
            
            new_fp.undistorted_point << undistorted_kp.pt.x, undistorted_kp.pt.y;
            new_fp.normalized_point << (undistorted_kp.pt.x-cx)/fx, (undistorted_kp.pt.y-cy)/fy, 1.0;

            // cv::KeyPoint undistorted_kp_t;
            // cv::KeyPoint kp_t;
            // kp_t.pt.x = new_fp.image_point[0];
            // kp_t.pt.y = new_fp.image_point[1];

            // undistortion_ptr_->DistortionKeyPoint(kp_t, undistorted_kp_t);
            
            // LOG(INFO) << " undistorted_kp ======= : " << undistorted_kp.pt.x << " , " << undistorted_kp.pt.y << std::endl;
            // LOG(INFO) << " undistorted_kp_t ===== : " << undistorted_kp_t.pt.x << " , " << undistorted_kp_t.pt.y << std::endl;

            // Vector2d predict_undistorted_point;
            // predict_undistorted_point << fx*new_fp.normalized_point[0]+cx, fy*new_fp.normalized_point[1]+cy;

            // LOG(INFO) << " predict_undistorted_point ===== : " << predict_undistorted_point.transpose() << std::endl;
            // LOG(INFO) << " new_fp.preundistorted_point ===== : " << new_fp.undistorted_point.transpose() << std::endl;

            // LOG(INFO) << " new_fp.normalized_point ===== : " << new_fp.normalized_point.transpose() << std::endl;

            feature_points.push_back(new_fp);
        }
    }

    return feature_points;
}

void read_data::read_feature_txt(std::vector<feature::FeaturePoint>& feature_data, std::vector<cv::KeyPoint>& undistorted_kps, const Eigen::Matrix3d& intrinsic_matrix, const std::string& filename) {
    std::vector<FeatureOutput> results; 
    
    static int frame_count = -1;

    // const std::string filename = path_ + "feature/frame_" + std::to_string(++frame_count) + ".txt";
    LoadFrameTxtData(results, undistorted_kps, filename);
        
    // LOG(INFO) << "results " << results.size() << std::endl;
    // LOG(INFO) << "undistorted_kps " << undistorted_kps.size() << std::endl;
    
    if(undistorted_kps.empty()) {
        results.clear();
        return;
    }
    feature_data = ConvertTxtFeatureOutputToFeaturePoints(results, undistorted_kps, intrinsic_matrix);  
}
